
#include "reflector_detector.hpp"

using std::placeholders::_1;
using namespace std::chrono_literals;

namespace reflector_slam_system {

// ======== Ceres 残差：点到圆距离 - r ========
struct CircleResidual {
  explicit CircleResidual(double x, double y) : x_(x), y_(y) {}
  template<typename T>
  bool operator()(const T* const c, T* residual) const {
    // c = [cx, cy, r]
    T dx = T(x_) - c[0];
    T dy = T(y_) - c[1];
    residual[0] = ceres::sqrt(dx*dx + dy*dy) - c[2];
    return true;
  }
  double x_, y_;
};

// ================== 预处理/筛选 ==================
void ReflectorDetector::passThroughZ(CloudT::Ptr cloud)
{
  //printf("进入 passThroughZ......");
  pcl::PassThrough<PointT> pass;
  pass.setInputCloud(cloud);
  pass.setFilterFieldName("z");
  pass.setFilterLimits(params_.z_min_, params_.z_max_);
  pass.filter(*cloud);

  CloudT tmp; tmp.reserve(cloud->size());
  for (auto &p : *cloud) {
    const double r = std::hypot(p.x, p.y);
    if (r >= params_.range_min_ && r <= params_.range_max_) tmp.push_back(p);
  }
  cloud->swap(tmp);
}

void ReflectorDetector::coarseFilter(const CloudT::Ptr& in, CloudT::Ptr out, std::vector<size_t>& kept_idx)
{
  //printf("进入 coarseFilter......");

  // 用两点式直线 I = a + b*r
  const double b = (params_.coarse_i1_ - params_.coarse_i0_) / std::max(1e-6, (params_.coarse_r1_ - params_.coarse_r0_));
  const double a = params_.coarse_i0_ - b * params_.coarse_r0_;

  out->clear(); out->reserve(in->size());
  kept_idx.clear(); kept_idx.reserve(in->size());

  for (size_t i = 0; i < in->size(); ++i) {
    const auto& p = (*in)[i];
    const double r = std::hypot(p.x, p.y);
    const double thr = a + b * r;
    if (p.intensity >= thr) {
      out->push_back(p);
      kept_idx.push_back(i);
    }
  }
}

int ReflectorDetector::calculateMuAndSd(const CloudT::Ptr& in,
                                            std::vector<double>& mu, std::vector<double>& sd,
                                            double& rmin, double& rmax)
{
  if (in->empty()) { mu.clear(); sd.clear(); rmin = 0; rmax = 0; return 0; }

  rmin = std::numeric_limits<double>::infinity();
  rmax = -std::numeric_limits<double>::infinity();

  for (const auto& p : *in) {
    const double r = std::hypot(p.x, p.y);
    rmin = std::min(rmin, r);
    rmax = std::max(rmax, r);
  }   // 找出点云中的最小、最大距离

  const int nb = std::max(1, (int)std::ceil((rmax - rmin) / std::max(0.1, params_.bin_size_)));    // 桶数量
  std::vector<std::vector<float>> bins(nb);
  mu.assign(nb, 0.0);
  sd.assign(nb, 0.0);

  for (const auto& p : *in) {
    const double r = std::hypot(p.x, p.y);
    const int bi = std::clamp((int)((r - rmin) / params_.bin_size_), 0, nb - 1);
    bins[bi].push_back(p.intensity);
  }

  //std::vector<double> mu(nb,0.0), sd(nb,0.0);  // 每桶的均值和方差向量
  for (int i=0;i<nb;++i){
    if ((int)bins[i].size() >= params_.min_points_per_bin_){
      double m=0; for (auto v:bins[i]) m+=v; m/=bins[i].size();    // 每桶求均值
      double v=0; for (auto v0:bins[i]){ double d=v0-m; v+=d*d; } v/=(bins[i].size()-1);   //每桶求方差
      mu[i]=m; sd[i]=std::sqrt(std::max(0.0,v));
    }else{
      mu[i]=1e9; sd[i]=0; // 小桶禁用
    }
  }
  return nb;
}

void ReflectorDetector::fineFilterInBins(const CloudT::Ptr& in, const std::vector<size_t>& /*kept_idx*/, std::vector<int>& mask_keep)
{
  //printf("进入 fineFilterInBins......");
  //printf("进入 fineFilterInBins......in_size 点云数量 = %zu\n", in->size());
  if (in->empty()) return;
  //printf("进入 1........ ");

  // 用粗筛后的点进行分桶统计（鲁棒且更接近“高反光”群）
  std::vector<double> mu, sd;  // 每桶的均值和方差向量
  double rmax,rmin;  //历史最大最小距离
  const int nb = calculateMuAndSd(in, mu, sd, rmin, rmax);
  //printf("进入 2.......点云数量 nb = %d\n", nb);

  if (nb <= 0) return;

  //printf("进入 3 .........");

  for (size_t i = 0; i < in->size(); ++i) {
    const auto& p = (*in)[i];
    const double r = std::hypot(p.x, p.y);
    const int bi = std::clamp((int)((r - rmin) / params_.bin_size_), 0, nb - 1);

    const bool bucket_ok = std::isfinite(mu[bi]) && (p.intensity >= params_.k_alpha_*(mu[bi] + params_.k_sigma_ * sd[bi]));      
    if (bucket_ok) {
      //printf("进入 4 .........");
      // 近距离复验：若距离很近，但强度低到连粗筛阈值 * ratio 都达不到，则丢弃（说明之前远处的“高强”是误判）
      /*
      if (enable_near_recheck_){
        double b = (coarse_i1_ - coarse_i0_) / std::max(1e-6, (coarse_r1_ - coarse_r0_));
        double a = coarse_i0_ - b * coarse_r0_;
        double r = std::hypot(p.x, p.y);
        double thr_coarse = a + b * r;
        if (r <= near_range_ && p.intensity < near_low_ratio_ * thr_coarse){
          mask_keep[i] = 0;
          continue;
        }
      }
      */
      mask_keep[i] = 1;
    }else{ 
      if (params_.enable_near_recheck_){
        double r = std::hypot(p.x, p.y);
        double thr_coarse = params_.near_i_trapezoid_ - params_.near_low_ratio_ * r;
        
        if (r <= params_.near_range_ && p.intensity >   thr_coarse){
          mask_keep[i] = 1;
          continue;
        }
      }  

      mask_keep[i] = 0;
    }
  }

  // 调试用，后删掉
  int count = 0;
  for(auto i : mask_keep){
    count += i;
  }
  printf("打印高反点的数量： count=%d\n", count);
  

}

// ================== 圆拟合/聚类 ==================
bool ReflectorDetector::fitCircleCeres(const CloudT::Ptr& cloud, const std::vector<int>& idx,
                                           Eigen::Vector2d& center, double& radius, double& avg_I) const
{

  if ((int)idx.size() < params_.min_cluster_size_) return false;

  double mx = 0, my = 0; avg_I = 0;
  for (int i : idx) { mx += (*cloud)[i].x; my += (*cloud)[i].y; avg_I += (*cloud)[i].intensity; }
  mx /= idx.size(); my /= idx.size(); avg_I /= idx.size();

  double r0 = 0;
  for (int i : idx) {
    const double dx = (*cloud)[i].x - mx, dy = (*cloud)[i].y - my;
    r0 += std::sqrt(dx*dx + dy*dy);
  }
  r0 = std::max(1e-3, r0 / idx.size());

  double c[3] = {mx, my, std::max(1e-2, r0)};

  ceres::LossFunction* loss = new ceres::HuberLoss( /*delta*/ params_.circle_huber_delta_ );
  ceres::Problem problem;
  for (int i : idx) {
    ceres::CostFunction* cost = new ceres::AutoDiffCostFunction<CircleResidual, 1, 3>(
        new CircleResidual((*cloud)[i].x, (*cloud)[i].y));
    //double w = std::clamp(double((*cloud)[i].intensity), 1.0, 255.0); // 简单强度权
    problem.AddResidualBlock(cost, loss, c);
  }
  problem.SetParameterLowerBound(c, 2, 1e-3); // r > 0

  ceres::Solver::Options opts;
  opts.max_num_iterations = 50;
  opts.linear_solver_type = ceres::DENSE_QR;
  ceres::Solver::Summary sum;
  ceres::Solve(opts, &problem, &sum);

  center = Eigen::Vector2d(c[0], c[1]);
  radius = c[2];

  // 半径先验门（相对名义半径）
  if (params_.nominal_radius_ > 1e-6) {
    const double rel = std::abs(radius - params_.nominal_radius_) / params_.nominal_radius_;
    if (rel > params_.radius_gate_ratio_) return false;
  }

  // 内点率再检
  int inl = 0;
  for (int i : idx) {
    const double dx = (*cloud)[i].x - center.x();
    const double dy = (*cloud)[i].y - center.y();
    const double e  = std::abs(std::sqrt(dx*dx + dy*dy) - radius);
    if (e < params_.circle_inlier_eps_) ++inl;
  }
  return inl >= params_.min_cluster_size_;
}

void ReflectorDetector::clusterAndFit(const CloudT::Ptr& in, const std::vector<int>& mask_keep,
                                          std::vector<Eigen::Vector2d>& centers,
                                          std::vector<double>& radii,
                                          std::vector<double>& strengths)
{
  //printf("进入 clusterAndFit......");

  // 收集保留点
  CloudT::Ptr kept(new CloudT);
  kept->reserve(in->size());
  for (size_t i = 0; i < in->size(); ++i) {
    if (mask_keep[i]) kept->push_back((*in)[i]);
  }
  if (kept->empty()) return;

  // PCL Euclidean clustering
  pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
  tree->setInputCloud(kept);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<PointT> ec;
  ec.setClusterTolerance(params_.cluster_tolerance_);
  ec.setMinClusterSize(params_.min_cluster_size_);
  ec.setMaxClusterSize(params_.max_cluster_size_);
  ec.setSearchMethod(tree);
  ec.setInputCloud(kept);
  ec.extract(cluster_indices);

  centers.clear(); radii.clear(); strengths.clear();

  for (const auto& cidx : cluster_indices){

    //printf("打印每簇点的数量： %ld ",cidx.indices.size());
    
    std::vector<int> idx; idx.reserve(cidx.indices.size());
    for (int j : cidx.indices) idx.push_back(j);
    Eigen::Vector2d c; double r, I;
    if (fitCircleCeres(kept, idx, c, r, I)){
      centers.push_back(c);
      radii.push_back(r);
      strengths.push_back(I);

      //printf("打印反光柱圆心坐标： c.x()=%.3f , c.y()=%.3f , r= %.3f ,I= %.3f",c.x(), c.y() ,r ,I);
      
    }
  }
}
/*
// ================== 地标数据关联/维护 ==================
void ReflectorDetector::associateAndUpdate(const std::vector<Eigen::Vector2d>& centers,
                                               const std::vector<double>& radii,
                                               const std::vector<double>& strengths)
{
  std::vector<int> assignment(centers.size(), -1);

  std::vector<std::vector<double>> cost_matrix(centers.size());
  for (size_t i = 0; i < centers.size(); ++i) {
    auto& row = cost_matrix[i];
    row.reserve(params_.landmarks_.size());
    for (size_t j = 0; j < params_.landmarks_.size(); ++j) {
      const double distance    = (centers[i] - params_.landmarks_[j].center).norm();
      const double radius_diff = std::abs(radii[i] - params_.landmarks_[j].radius);
      row.push_back(distance + radius_diff);
    }
  }



  HungarianAlgorithm hungarian;
  hungarian.Solve(cost_matrix, assignment, params_.max_distance_threshold_);

  

  const auto now_t = this->now();

  for (size_t i = 0; i < centers.size(); ++i) {
    if (assignment[i] != -1) {
      const double alpha = 0.2;
      auto& lm = params_.landmarks_[assignment[i]];
      lm.center   = (1.0 - alpha) * lm.center + alpha * centers[i];
      lm.radius   = (1.0 - alpha) * lm.radius + alpha * radii[i];
      lm.strength = strengths[i];
      lm.hits++;
      lm.last_seen = now_t;
    } else {
      ReflectorLandmark lm;
      lm.center = centers[i];
      lm.radius = radii[i];
      lm.strength = strengths[i];
      lm.hits = 1;
      lm.last_seen = now_t;
      lm.id = "lm_" + std::to_string(params_.landmarks_.size());
      params_.landmarks_.push_back(std::move(lm));
    }
  }

  removeOldLandmarks(now_t);

  for(const auto& lm : params_.landmarks_){

    RCLCPP_INFO_THROTTLE(
        this->get_logger(), *this->get_clock(), 1000,
        "打印地标结果： lm.centers[i].x()=%.3f , lm.centers[i].y()=%.3f , lm.radius= %.3f",
        lm.center.x(), lm.center.y() ,lm.radius);
  }
  
}

void ReflectorDetector::removeOldLandmarks(const rclcpp::Time& current_time)
{
  auto it = params_.landmarks_.begin();
  while (it != params_.landmarks_.end()) {
    if ((current_time - it->last_seen).seconds() > params_.max_age_) {
      RCLCPP_INFO(this->get_logger(), "Removing outdated landmark: %s", it->id.c_str());
      it = params_.landmarks_.erase(it);
    } else {
      if ((current_time - it->last_seen).seconds() > params_.decay_threshold_) {
        it->hits = std::max(0, it->hits - static_cast<int>(params_.decay_factor_));
      }
      ++it;
    }
  }
  params_.landmarks_.erase(
    std::remove_if(params_.landmarks_.begin(), params_.landmarks_.end(),
                   [this](const ReflectorLandmark& lm){ return lm.hits < params_.min_hits_; }),
    params_.landmarks_.end());
}

// ================== 坐标系转换/发布可视化 ==================
bool ReflectorDetector::transformPointToMap(double x_laser, double y_laser,
                                                const geometry_msgs::msg::TransformStamped tf,
                                                double& x_map, double& y_map)
{

  try {
      tf2::doTransform(in, out, tf);
      return true;
  } catch (const tf2::TransformException& ex) {
    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000,
                         "TF transform failed (%s <- %s): %s",
                         params_.target_frame_.c_str(), params_.laser_frame_.c_str(), ex.what());
    return false;
  }
}



bool ReflectorDetector::transformPointToMap(double x_laser, double y_laser, const rclcpp::Time& stamp,
                                                double& x_map, double& y_map)
{
  try {
    // 以点云时间戳查 map <- laser
    const auto timeout = std::chrono::milliseconds(500);
    if (!tf_buffer_->canTransform(target_frame_, laser_frame_, tf2::TimePointZero, timeout)) {
      RCLCPP_WARN(this->get_logger(), "Waiting for TF %s <- %s ...",
                  target_frame_.c_str(), laser_frame_.c_str());
      return false;
    }

    geometry_msgs::msg::TransformStamped tf =
        tf_buffer_->lookupTransform(target_frame_, laser_frame_, stamp, 100ms);

    geometry_msgs::msg::PointStamped in, out;
    in.header.frame_id = laser_frame_;
    in.header.stamp    = stamp;
    in.point.x = x_laser; in.point.y = y_laser; in.point.z = 0.0;

    tf2::doTransform(in, out, tf);
    x_map = out.point.x;
    y_map = out.point.y;
    return true;
  } catch (const tf2::TransformException& ex) {
    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000,
                         "TF transform failed: %s", ex.what());
    return false;
  }
}

// 发布地标信息
void ReflectorDetector::publishLandmarksInMap(const geometry_msgs::msg::TransformStamped tf,
                                                  const std::vector<Eigen::Vector2d>& centers)
{
  cartographer_ros_msgs::msg::LandmarkList ls;
  ls.header.frame_id = params_.target_frame_;
  ls.header.stamp = tf.stamp;

  visualization_msgs::msg::MarkerArray arr;

  for (size_t i = 0; i < centers.size(); ++i) {
    double xm = 0.0, ym = 0.0;
    if (!transformPointToMap(centers[i].x(), centers[i].y(), tf, xm, ym)) continue;

    cartographer_ros_msgs::msg::LandmarkEntry e;
    e.id = "refl_" + std::to_string(i);
    e.tracking_from_landmark_transform.position.x = xm;  //修改centers[i].x()
    e.tracking_from_landmark_transform.position.y = ym;  //修改centers[i].y()
    e.tracking_from_landmark_transform.position.z = 0.0;
    e.tracking_from_landmark_transform.orientation.w = 1.0;
    e.translation_weight = params_.lm_translation_weight_;
    e.rotation_weight = params_.lm_rotation_weight_;//地标变换中平移部分的权重（即对平移的可靠性或重要性的评估）。权重越高，表示该平移部分越重要。这个权重通常在地图优化或地图匹配中使用，用来调整平移部分的影响力
    ls.landmarks.push_back(e);

    // 创建并添加 Marker，用于 RViz 显示
    visualization_msgs::msg::Marker m;
    m.header.frame_id = params_.target_frame_;
    m.header.stamp = tf.stamp;
    m.ns = params_.marker_ns_;
    m.id = static_cast<int>(i);
    m.type = visualization_msgs::msg::Marker::CYLINDER;
    m.action = visualization_msgs::msg::Marker::ADD;
    m.pose.position.x = xm;   //修改centers[i].x()
    m.pose.position.y = ym;   //修改centers[i].y()
    m.pose.position.z = 0.0;
    m.pose.orientation.w = 1.0;
    //m.scale.x 和 m.scale.y 定义了圆的直径（这两个值相等，表示一个圆形）

    const double dia = std::max(0.05, 2.0 * params_.nominal_radius_);
    m.scale.x = 5*dia; m.scale.y = 5*dia; m.scale.z = 0.5;
    m.color.r = 1.0; 
    m.color.g = 0.8 - 0.5 * (centers[i].norm() / 255.0);  // 根据强度调整颜色
    m.color.b = 0.2; 
    m.color.a = 0.9;
    m.lifetime = rclcpp::Duration::from_seconds(0.5);
    arr.markers.push_back(m);

    RCLCPP_INFO_THROTTLE(
        this->get_logger(), *this->get_clock(), 1000,
        "发布函数中反光柱：centers[i].x=%.3f , centers[i].y=%.3f ",
        centers[i].x(), centers[i].y() );
  }
    
  pub_markers_->publish(arr);  // 发布 Marker Array
  pub_landmarks_->publish(ls);  // 发布 Landmark List
}

// ================== 终端打印：全局强度-距离极值 ==================
void ReflectorDetector::printGlobalIntensityDistanceStats(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) 
{

  CloudT::Ptr cloud(new CloudT);
  pcl::fromROSMsg(*msg, *cloud);
  // 更新并打印“到目前为止”的全局 rmin/rmax 及其强度
  for (const auto& p : *cloud) {
    const double r = std::hypot(p.x, p.y);
    if (r < params_.global_rmin_) { params_.global_rmin_ = r; params_.global_rmin_intensity_ = p.intensity; }
    if (r > params_.global_rmax_) { params_.global_rmax_ = r; params_.global_rmax_intensity_ = p.intensity; }
  }

  if (!std::isfinite(params_.global_rmin_) || !std::isfinite(params_.global_rmax_)) return;
  RCLCPP_INFO_THROTTLE(
      this->get_logger(), *this->get_clock(), 1000,
      "累计强度-距离统计：r_min=%.3f (I=%.1f), r_max=%.3f (I=%.1f)",
      params_.global_rmin_, params_.global_rmin_intensity_, params_.global_rmax_, params_.global_rmax_intensity_);
}
*/
} // namespace reflector_slam_system

